home *** CD-ROM | disk | FTP | other *** search
/ AmigActive 23 / AACD 23.iso / AACD / Programming / tek / examples / kine / mathutil.c < prev    next >
C/C++ Source or Header  |  2001-05-26  |  12KB  |  645 lines
  1. /*
  2.  *
  3.  *    Math Funktions 
  4.  *    Frank Pagels Defect Softworks 2001
  5.  *
  6.  *
  7.  */
  8.  
  9. #include <stdio.h>
  10. #include <math.h>
  11. #include <string.h>
  12. #include "mathutil.h"
  13. #include <stdlib.h>
  14.  
  15. #ifndef PI
  16.     #ifdef M_PI
  17.     #define PI M_PI
  18.     #else
  19.     #define PI 3.1415927
  20.     #endif
  21. #endif
  22.  
  23. #define CM_0 OF_11
  24. #define CM_1 OF_12
  25. #define CM_2 OF_13
  26. #define CM_3 OF_21
  27. #define CM_4 OF_22
  28. #define CM_5 OF_23
  29. #define CM_6 OF_31
  30. #define CM_7 OF_32
  31. #define CM_8 OF_33
  32.  
  33.  
  34. /*
  35. ** Computes the determinant of the upper 3x3 submatrix block
  36. ** of the supplied matrix
  37. */
  38. TFLOAT Determinant(Matrix *pA)
  39. {
  40.     #define a(x) (pA->v[CM_##x])
  41.      return ( a(0)*a(4)*a(8)  +  a(1)*a(5)*a(6)  + a(2)*a(3)*a(7)
  42.        -  a(0)*a(5)*a(7)  -  a(1)*a(3)*a(8)  - a(2)*a(4)*a(6));
  43.     #undef a
  44. }
  45.  
  46. /*
  47. ** Builds the inverse of the upper 3x3 submatrix block of the
  48. ** supplied matrix for the backtransformation of the normals
  49. */
  50. void DoInvert(Matrix *pA, Matrix *pB)
  51. {
  52.     TFLOAT det = 1.0 / Determinant(pA);
  53.  
  54.     #define a(x) (pA->v[CM_##x])
  55.     #define b(x) (pB->v[CM_##x])
  56.  
  57.     b(0) = (TFLOAT)(a(4)*a(8) - a(5)*a(7))*det;
  58.     b(1) = (TFLOAT)(a(2)*a(7) - a(1)*a(8))*det;
  59.     b(2) = (TFLOAT)(a(1)*a(5) - a(2)*a(4))*det;
  60.  
  61.     b(3) = (TFLOAT)(a(5)*a(6) - a(3)*a(8))*det;
  62.     b(4) = (TFLOAT)(a(0)*a(8) - a(2)*a(6))*det;
  63.     b(5) = (TFLOAT)(a(2)*a(3) - a(0)*a(5))*det;
  64.     
  65.     b(6) = (TFLOAT)(a(3)*a(7) - a(4)*a(6))*det;
  66.     b(7) = (TFLOAT)(a(1)*a(6) - a(0)*a(7))*det;
  67.     b(8) = (TFLOAT)(a(0)*a(4) - a(1)*a(3))*det;
  68.  
  69.     #undef a
  70.     #undef b
  71.  
  72. }
  73.  
  74. /*
  75. **
  76. **    Simple Matrixmultiplikation
  77. **  A * B = C
  78. **
  79. */
  80.  
  81. void MatMultGeneral(Matrix *pA, Matrix *pB, Matrix *pC)
  82. {
  83.     #define a(x) (pA->v[OF_##x])
  84.     #define b(x) (pB->v[OF_##x])
  85.     #define c(x) (pC->v[OF_##x])
  86.  
  87.  
  88.     c(11) = a(11)*b(11)+a(12)*b(21)+a(13)*b(31)+a(14)*b(41);
  89.     c(12) = a(11)*b(12)+a(12)*b(22)+a(13)*b(32)+a(14)*b(42);    
  90.     c(13) = a(11)*b(13)+a(12)*b(23)+a(13)*b(33)+a(14)*b(43);    
  91.     c(14) = a(11)*b(14)+a(12)*b(24)+a(13)*b(34)+a(14)*b(44);    
  92.  
  93.     c(21) = a(21)*b(11)+a(22)*b(21)+a(23)*b(31)+a(24)*b(41);
  94.     c(22) = a(21)*b(12)+a(22)*b(22)+a(23)*b(32)+a(24)*b(42);    
  95.     c(23) = a(21)*b(13)+a(22)*b(23)+a(23)*b(33)+a(24)*b(43);    
  96.     c(24) = a(21)*b(14)+a(22)*b(24)+a(23)*b(34)+a(24)*b(44);    
  97.  
  98.     c(31) = a(31)*b(11)+a(32)*b(21)+a(33)*b(31)+a(34)*b(41);
  99.     c(32) = a(31)*b(12)+a(32)*b(22)+a(33)*b(32)+a(34)*b(42);    
  100.     c(33) = a(31)*b(13)+a(32)*b(23)+a(33)*b(33)+a(34)*b(43);    
  101.     c(34) = a(31)*b(14)+a(32)*b(24)+a(33)*b(34)+a(34)*b(44);    
  102.  
  103.     c(41) = a(41)*b(11)+a(42)*b(21)+a(43)*b(31)+a(44)*b(41);
  104.     c(42) = a(41)*b(12)+a(42)*b(22)+a(43)*b(32)+a(44)*b(42);    
  105.     c(43) = a(41)*b(13)+a(42)*b(23)+a(43)*b(33)+a(44)*b(43);    
  106.     c(44) = a(41)*b(14)+a(42)*b(24)+a(43)*b(34)+a(44)*b(44);    
  107.  
  108.     #undef a
  109.     #undef b
  110.     #undef c
  111. }
  112.  
  113. /*
  114. **    A * B , A=4x4 , B = 1x4
  115. **
  116. */
  117.  
  118. void MatMultPoint(Matrix *pA, TFLOAT *v, TFLOAT *tmp)
  119. {
  120.     #define a(x) (pA->v[OF_##x])
  121.  
  122.     tmp[0] = a(11) * v[0] + a(12) * v[1] + a(13) * v[2] + a(14) * v[3];
  123.     tmp[1] = a(21) * v[0] + a(22) * v[1] + a(23) * v[2] + a(24) * v[3];
  124.     tmp[2] = a(31) * v[0] + a(32) * v[1] + a(33) * v[2] + a(34) * v[3];
  125.     tmp[3] = a(41) * v[0] + a(42) * v[1] + a(43) * v[2] + a(44) * v[3];
  126.  
  127.     #undef a
  128. }
  129.  
  130.  
  131. /*
  132. ** Copy matrix B to matrix A
  133. ** A = B
  134. */
  135. void MatCopy(Matrix *pA, Matrix *pB)
  136. {
  137.     int i;
  138.  
  139.     for (i=0; i<16; i++) pA->v[i] = pB->v[i];
  140.     pA->flags = pB->flags;
  141.     pA->Inverse = pB->Inverse;
  142. }
  143.  
  144.  
  145. void LoadMatrix(Matrix *pA, const TFLOAT *v)
  146. {
  147.     #define a(x) pA->v[OF_##x] = *v++
  148.  
  149.     a(11); a(21); a(31); a(41);
  150.     a(12); a(22); a(32); a(42);
  151.     a(13); a(23); a(33); a(43);
  152.     a(14); a(24); a(34); a(44);
  153.  
  154.     //if (*(v-4) == 0.f && *(v-3) == 0.f && *(v-2) == 0.f && *(v-1) == 1.f)
  155.     //    pA->flags = MGLMAT_0001;
  156.  
  157.     #undef a
  158. }
  159.  
  160. void LoadIdentity(Matrix *pA)
  161. {
  162.     #define a(x) pA->v[OF_##x] = 0.f;
  163.     #define b(x) pA->v[OF_##x] = 1.f;
  164.  
  165.     b(11); a(21); a(31); a(41);
  166.     a(12); b(22); a(32); a(42);
  167.     a(13); a(23); b(33); a(43);
  168.     a(14); a(24); a(34); b(44);
  169.  
  170.     //pA->flags = MGLMAT_IDENTITY;
  171.  
  172.     #undef a
  173.     #undef b
  174. }
  175.  
  176.  
  177. void PrintMatrix(Matrix *pA)
  178. {
  179.     #define a(x) (pA->v[OF_##x])
  180.     printf("Matrix at 0x%lX\n", (TUINT)pA);
  181.     printf("    | %6.3f %6.3f %6.3f %6.3f |\n",
  182.         a(11), a(12), a(13), a(14));
  183.     printf("    | %6.3f %6.3f %6.3f %6.3f |\n",
  184.         a(21), a(22), a(23), a(24));
  185.     printf("A = | %6.3f %6.3f %6.3f %6.3f |\n",
  186.         a(31), a(32), a(33), a(34));
  187.     printf("    | %6.3f %6.3f %6.3f %6.3f |\n",
  188.         a(41), a(42), a(43), a(44));
  189.     #undef a
  190. }
  191.  
  192. /*
  193. **    Get Hartenberg Matrix for
  194. **    Theta, d, a, alpha
  195. **
  196. */
  197. void GetHartenberg(joint *j, Matrix *pA)
  198. {
  199.     TFLOAT ct,st,cal,sal,th,al;
  200.     
  201.     #define a(x) (pA->v[OF_##x])
  202.  
  203.     th = j->theta;     /* * PI / 180; */
  204.     al = j->alpha;     /* * PI / 180; */
  205.  
  206.     ct = cos(th);
  207.     st = sin(th);
  208.     cal = cos(al);
  209.     sal = sin(al);    
  210.  
  211.     a(11) = ct;
  212.     a(12) = -cal*st;
  213.     a(13) = sal*st;
  214.     a(14) = j->a*ct;
  215.     
  216.     a(21) = st;
  217.     a(22) = cal*ct;
  218.     a(23) = -sal*ct;
  219.     a(24) = j->a*st;
  220.     
  221.     a(31) = 0;
  222.     a(32) = sal;
  223.     a(33) = cal;
  224.     a(34) = j->d;
  225.     
  226.     a(41) = 0;
  227.     a(42) = 0;
  228.     a(43) = 0;
  229.     a(44) = 1;
  230.     
  231.     #undef a    
  232.  
  233. }
  234.  
  235. /*
  236. **
  237. **    Subtract C = A - B
  238. **
  239. */
  240. void GenMatSub(GenMatrix *A,GenMatrix *B, GenMatrix *C)
  241. {
  242.     TINT i,j;
  243.     
  244.     for(i=0;i < A->rows;i++)
  245.     {
  246.         for(j=0;j < A->colum;j++)
  247.         {
  248.             C->m[i][j] = A->m[i][j] - B->m[i][j];
  249.         }
  250.     }
  251.  
  252. }
  253.  
  254. /*
  255. **
  256. **    Add A + B = C
  257. **
  258. */
  259. void GenMatAdd(GenMatrix *A, GenMatrix *B, GenMatrix *C)
  260. {
  261.     TINT i,j;
  262.     
  263.     for(i=0;i < A->rows;i++)
  264.     {
  265.         for(j=0;j < A->colum;j++)
  266.         {
  267.             C->m[i][j] = A->m[i][j] + B->m[i][j];
  268.         }
  269.     }
  270.  
  271. }
  272.  
  273. /*
  274. **
  275. **    Multiply Matrix A,B to C
  276. **
  277. */
  278.  
  279. void GenMatMultiply(GenMatrix *A, GenMatrix *B, GenMatrix *C)
  280. {
  281.     TINT i,j,k;
  282.     TFLOAT sum;
  283.     
  284.     if(A->colum == B->rows)
  285.     {
  286.         for(i=0;i < A->rows;i++)
  287.         {
  288.             for(k=0; k < B->colum;k++)
  289.             {
  290.                 C->m[i][k] = 0;            
  291.                 for(j=0;j < A->colum;j++)
  292.                 {
  293.                     C->m[i][k] += A->m[i][j] * B->m[j][k];
  294.                 }
  295.             }
  296.         }
  297.     }
  298. }
  299.  
  300.  
  301. /*
  302. **
  303. **    Transpose Matrix A to B
  304. **
  305. */
  306. void GenMatTranspose(GenMatrix *A, GenMatrix *B)
  307. {
  308.     TINT i,j;
  309.     
  310.     if(A->colum == B->rows)
  311.     {
  312.         if(A->rows == B->colum)
  313.         {
  314.             for(i=0;i < A->rows;i++)
  315.             {
  316.                 for(j=0;j < A->colum;j++)
  317.                 {
  318.                     B->m[j][i] = A->m[i][j];
  319.                 }
  320.             }
  321.         }
  322.     }    
  323.  
  324. }
  325.  
  326.  
  327. /*
  328. **
  329. **    GenLoadIndentity
  330. **
  331. */
  332. void GenMatLoadIdentity(GenMatrix *A, TFLOAT a)
  333. {
  334.     TINT i,j;
  335.     
  336.     memset(A->m,0,A->rows*A->colum*sizeof(TFLOAT));
  337.         
  338.     if(A->colum == A->rows)
  339.     {
  340.         for(i=0;i < A->rows;i++)
  341.         {
  342.             for(j=0;j < A->colum;j++)
  343.             {
  344.                 if(i==j)
  345.                 A->m[i][j] = a;
  346.                 else
  347.                 A->m[i][j] = 0;
  348.             }
  349.         }
  350.     }
  351. }
  352.  
  353.  
  354. /*
  355. **
  356. **    Invers Matrix with Crout's Method
  357. **
  358. */
  359.  
  360. void GenMatInvers1(GenMatrix *A)
  361. {
  362.     TINT i,j,k;    
  363.     TINT N = A->rows;
  364.     TFLOAT a,sum;
  365.     
  366.     
  367.     /* LU Decomposing with Crout's Method */
  368.     
  369.     a = 1.0 / A->m[0][0];
  370.     
  371.     for(j=1;j < N ;j++)
  372.     {
  373.         A->m[0][j]  = A->m[0][j] * a;
  374.     }
  375.  
  376.     for(j=1;j<N-1;j++)
  377.     {
  378.         i = j;
  379.         for(i;i<N ;i++)
  380.         {
  381.             sum = 0;    
  382.             for(k=0;k < j;k++)
  383.             {
  384.                 sum += A->m[i][k] * A->m[k][j];
  385.             }
  386.             
  387.             A->m[i][j] -= sum;
  388.         }
  389.  
  390.         k=j+1;
  391.         for(k;k < N;k++)
  392.         {
  393.             sum = 0;
  394.             for(i=0;i < j;i++)
  395.             {
  396.                 sum += A->m[j][i] * A->m[i][k];
  397.             }
  398.  
  399.             A->m[j][k] -= sum;
  400.             A->m[j][k] /= A->m[j][j];
  401.         }
  402.     }
  403.         
  404.  
  405.     for(k=0;k < N-1;k++)
  406.     {
  407.         sum += A->m[N-1][k] * A->m[k][N-1];
  408.     }
  409.     
  410.     A->m[N-1][N-1] -= sum; 
  411.  
  412.     /* */
  413.     /*
  414.     for(i=1;j<=N;j++)
  415.     {
  416.         for(i=1;i<=N;i++) col[i]=0.0;
  417.         col[j]=1.0;
  418.         lubks(&A,N,indx,col);
  419.         for(i=1;i<=N,j++)
  420.     }
  421.     */
  422.  
  423. }
  424.  
  425. /*
  426. **
  427. ** Invert Matrix nach GemsII Rod G. Bogart
  428. **
  429. */
  430. void GenMatInvers(GenMatrix *A)
  431. {
  432.     TINT i,j,k;
  433.                     /* Locations of pivot elements */
  434.     TINT *pvt_i, *pvt_j;
  435.     TFLOAT pvt_val;                     /* Value of current pivot element */
  436.     TFLOAT hold;                        /* Temporary storage */
  437.     TFLOAT determ;                      /* Determinant */
  438.  
  439.     determ = 1.0;
  440.  
  441.     //pvt_i = (TINT *) malloc(A->rows * sizeof(TINT));
  442.     //pvt_j = (TINT *) malloc(A->rows * sizeof(TINT));
  443.     pvt_i = A->pvt_i;
  444.     pvt_j = A->pvt_j;
  445.     
  446.     for (k = 0; k < A->rows; k++)
  447.     {
  448.         /* Locate k'th pivot element */
  449.         pvt_val = A->m[k][k];            /* Initialize for search */
  450.         pvt_i[k] = k;
  451.         pvt_j[k] = k;
  452.         for (i = k; i < A->rows; i++)
  453.           for (j = k; j < A->rows; j++)
  454.             if (fabs(A->m[i][j]) > fabs(pvt_val))
  455.             {
  456.                 pvt_i[k] = i;
  457.                 pvt_j[k] = j;
  458.                 pvt_val = A->m[i][j];
  459.             }
  460.         /* Product of pivots, gives determinant when finished */
  461.         determ *= pvt_val;
  462.         if (determ == 0.0) {    
  463.          /* Matrix is singular (zero determinant). */
  464.         free(pvt_i);
  465.         free(pvt_j);
  466.             return;
  467.     }
  468.  
  469.         /* "Interchange" rows (with sign change stuff) */
  470.         i = pvt_i[k];
  471.         if (i != k)                     /* If rows are different */
  472.           for (j = 0; j < A->rows; j++)
  473.           {
  474.             hold = -A->m[k][j];
  475.             A->m[k][j] = A->m[i][j];
  476.             A->m[i][j] = hold;
  477.           }
  478.  
  479.         /* "Interchange" columns */
  480.         j = pvt_j[k];
  481.         if (j != k)                     /* If columns are different */
  482.           for (i = 0; i < A->rows; i++)
  483.           {
  484.             hold = -A->m[i][k];
  485.             A->m[i][k] = A->m[i][j];
  486.             A->m[i][j] = hold;
  487.           }
  488.         /* Divide column by minus pivot value */
  489.         for (i = 0; i < A->rows; i++)
  490.           if (i != k)                   /* Don't touch the pivot entry */
  491.             A->m[i][k] /= ( -pvt_val) ;  /* (Tricky C syntax for division) */
  492.  
  493.         /* Reduce the matrix */
  494.         for (i = 0; i < A->rows; i++)
  495.         {
  496.             hold = A->m[i][k];
  497.             for (j = 0; j < A->rows; j++)
  498.               if ( i != k && j != k )   /* Don't touch pivot. */
  499.                 A->m[i][j] += hold * A->m[k][j];
  500.         }
  501.  
  502.         /* Divide row by pivot */
  503.         for (j = 0; j < A->rows; j++)
  504.           if (j != k)                   /* Don't touch the pivot! */
  505.             A->m[k][j] /= pvt_val;
  506.  
  507.         /* Replace pivot by reciprocal (at last we can touch it). */
  508.         A->m[k][k] = 1.0/pvt_val;
  509.     }
  510.  
  511.     /* That was most of the work, one final pass of row/column interchange */
  512.     /* to finish */
  513.     for (k = A->rows-2; k >= 0; k--)  /* Don't need to work with 1 by 1 */
  514.                                         /* corner */
  515.     {
  516.         i = pvt_j[k];         /* Rows to swap correspond to pivot COLUMN */
  517.         if (i != k)                     /* If rows are different */
  518.           for(j = 0; j < A->rows; j++)
  519.           {
  520.             hold = A->m[k][j];
  521.             A->m[k][j] = -A->m[i][j];
  522.             A->m[i][j] = hold;
  523.           }
  524.  
  525.         j = pvt_i[k];           /* Columns to swap correspond to pivot ROW */
  526.         if (j != k)                     /* If columns are different */
  527.           for (i = 0; i < A->rows; i++)
  528.           {
  529.             hold = A->m[i][k];
  530.             A->m[i][k] = -A->m[i][j];
  531.             A->m[i][j] = hold;
  532.           }
  533.     }
  534.  
  535.     //free(pvt_i);
  536.     //free(pvt_j);
  537.     //return(determ);
  538. }
  539.  
  540.  
  541. /*
  542. **
  543. **    Copy GenMatrix A to B
  544. **
  545. */
  546. void GenMatCopy(GenMatrix *A,GenMatrix *B)
  547. {
  548.     TINT i,j;
  549.     
  550.     for(i=0;i<A->rows;i++)
  551.     {
  552.         for(j=0;j<A->colum;j++)
  553.         {
  554.             B->m[i][j] = A->m[i][j];
  555.         }
  556.     }
  557. }
  558.  
  559. /*
  560. **
  561. **    Get Pseudoinverse of A
  562. **
  563. */
  564. void GenMatPseudoInvers(GenMatrix *A, GenMatrix *B, GenMatrix *C)
  565. {
  566.     
  567.     if(A->rows > A->colum)
  568.     {
  569.         GenMatTranspose(A,B);
  570.         GenMatMultiply(B,A,C);
  571.         GenMatInvers(C);
  572.         GenMatMultiply(A,C,B);
  573.         GenMatTranspose(B,A);
  574.     }
  575.     else
  576.     {
  577.         GenMatTranspose(A,B);
  578.         GenMatMultiply(A,B,C);
  579.         GenMatInvers(C);
  580.         A->rows = B->rows;
  581.         A->colum = B->colum;
  582.         GenMatMultiply(B,C,A);
  583.     }
  584. }        
  585.  
  586.         
  587. /*        
  588.     A = M;
  589.   if ( ROWS(M) > COLUMNS(M) ) 
  590.     A = TRANSPOSE(A);
  591.  
  592.   B = TRANSPOSE(A) * INV( A * TRANSPOSE(A) );
  593.  
  594.   if ( ROWS(M) > COLUMNS(M) )
  595.     B = TRANSPOSE(B);
  596. */
  597.  
  598.  
  599. /*
  600. **
  601. **    Print Matrix
  602. **
  603. */
  604. void GenPrintMatrix(GenMatrix *A)
  605. {
  606.     TINT i,j;
  607.     
  608.     for(i=0;i < A->rows;i++)
  609.     {
  610.         printf("   |");
  611.         for(j=0;j < A->colum;j++)
  612.         {
  613.             printf("  %6.3f",A->m[i][j]);
  614.         }
  615.         printf("|\n");
  616.     }
  617.     printf("\n\n");
  618. }
  619.  
  620. void InitGenMatrix(GenMatrix *A, TINT row, TINT colum)
  621. {
  622.     
  623.     //memset(A->m,0,row*colum*sizeof(TFLOAT));
  624.     A->rows=row;
  625.     A->colum=colum;
  626.     
  627. }
  628.  
  629. void DestroyGenMatrix(GenMatrix *A)
  630. {
  631.     A->rows = 0;
  632.     A->colum = 0;
  633. }
  634.  
  635. void GenLoadMatrix(Matrix *pA, const TFLOAT *v)
  636. {
  637.     
  638. }
  639.  
  640.  
  641.  
  642.  
  643.  
  644.  
  645.